import threading
import RPi.GPIO as GPIO
import time
import os
import numpy as np
import inspect
import ctypes
from tensorflow.keras.applications.mobilenet_v2 import preprocess_input
from tensorflow.keras.preprocessing import image
import tensorflow as tf
import cv2

num = 1

class garsorting:
    def __init__(self):
        self.model = tf.keras.models.load_model("./model_garbage_ResNet50.h5") # model path
        print('init success')
        #self.result = 1
    def sort(self):
        # 0,1,2,3-----chuyu,kehui,qita,youhai
        label = ['其他垃圾_一次性杯子', '其他垃圾_一次性棉签', '其他垃圾_创可贴', '其他垃圾_卫生纸', '其他垃圾_口罩', '其他垃圾_毛巾',\
                 '其他垃圾_眼镜', '其他垃圾_笔芯', '其他垃圾_除湿袋', '其他垃圾_餐巾纸', '厨余垃圾_梨', '厨余垃圾_橙子', '厨余垃圾_番茄',\
                 '厨余垃圾_白煮蛋', '厨余垃圾_白菜叶', '厨余垃圾_苹果', '厨余垃圾_茶叶', '厨余垃圾_草莓', '厨余垃圾_辣椒', '可回收物_书', \
                 '可回收物_保温杯', '可回收物_充电牙刷', '可回收物_剪刀', '可回收物_单车', '可回收物_双肩包','可回收物_吊牌', '可回收物_快递盒',\
                 '可回收物_拖鞋', '可回收物_椅子', '可回收物_纸袋', '可回收物_耳机', '可回收物_衣架', '可回收物_订书机', '可回收物_键盘',\
                 '有害垃圾_口服液瓶', '有害垃圾_灯泡', '有害垃圾_电池', '有害垃圾_纽扣电池', '有害垃圾_药品包装', '有害垃圾_药膏']
        #img = image.load_img(im_file,target_size=(224,224))
        flag,img=cv2.VideoCapture(0).read()
        cv2.imwrite('/home/pi/Desktop/re.jpg',img)
        img=cv2.resize(img,(224,224),interpolation=cv2.INTER_CUBIC)
        x = image.img_to_array(img)
        x2 = np.expand_dims(x,axis=0)
        #x2 = np.vstack([x2])
        x3=preprocess_input(x2)
        out = self.model.predict(x3)
        res = np.argmax(out)
        print(label[res])
        garclass = [2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,\
                    1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 3, 3, 3, 3, 3, 3]
        self.result = garclass[res]
        #self.result = 0


class Wheel:
    pins ={'a':[13,15],'b':[29,31],'c':[16,18],'d':[19,21]}
    def __init__(self,name):
        self.name = name
        self.pin = Wheel.pins[self.name]
        GPIO.setwarnings(False)
        GPIO.setmode(GPIO.BOARD)
        GPIO.setup(self.pin[0],GPIO.OUT)
        GPIO.setup(self.pin[1],GPIO.OUT)
        self.p1 = GPIO.PWM(self.pin[0],50)
        self.p2 = GPIO.PWM(self.pin[1],50)
        GPIO.output(self.pin[0],False)
        GPIO.output(self.pin[1],False)
        self.stop()
    def forward(self,speed):
        self.p1.start(speed)
    def stop(self):
        GPIO.output(self.pin[0],False)
        GPIO.output(self.pin[1],False)
        self.p1.stop()
        self.p2.stop()
    def back(self,speed):
        self.p2.start(speed)
    
    
class Car:
    wheel=[Wheel('a'),Wheel('b'),Wheel('c'),Wheel('d')] 
    @staticmethod
    def init():
        GPIO.setmode(GPIO.BOARD)   
        print('The car is ready!')
    @staticmethod
    def forward(speed = 100):
        print('go straight forward in '+str(speed))
        Car.wheel[0].forward(speed - 0.3) 
        Car.wheel[1].forward(speed - 0.3)
        Car.wheel[3].forward(speed)
        Car.wheel[2].forward(speed)
    @staticmethod
    def left(speed = 100):
        print('turn left in '+str(speed))
        Car.wheel[0].forward(speed) 
        Car.wheel[1].forward(speed)
        Car.wheel[3].back(speed)
        Car.wheel[2].back(speed)
    @staticmethod
    def right(speed = 100):
        print('turn right in '+str(speed))
        Car.wheel[2].forward(speed) 
        Car.wheel[3].forward(speed)
        Car.wheel[0].back(speed)
        Car.wheel[1].back(speed)
    @staticmethod
    def back(speed = 100):
        print('go straight back in '+str(speed))
        Car.wheel[0].back(speed - 0.3) 
        Car.wheel[1].back(speed - 0.3)
        Car.wheel[3].back(speed)
        Car.wheel[2].back(speed)
    @staticmethod
    def stop(speed = 100):
        print('stop the car')
        for wheel in Car.wheel:
            wheel.stop() 
    @staticmethod
    def autofollow(speed = 100):
        print('error')
            

def findtar(obj, obj_tar = 0):
    find = []
    count = 0
    if len(obj) == 0:
        return 'notar'
    for temp in obj:
        if temp[0] == obj_tar :
            find.append(temp)
            count = count + 1
    if count == 0:
        return 'notar'
    else:
        return find


    

def automove(temp):       #move to the person
    width = 1
    w = chao()
    rd = 0
    if temp == 'notar':
        target = 2
    else : 
        target = temp[1]+temp[3]
    if target / width < 0.85:
        Car.left()
        t = 5*( 1 - target / width ) / 8
        time.sleep(t)
        Car.stop()
    elif target / width > 1.15:
        Car.right()
        t = 5*( target / width - 1 ) / 8
        time.sleep(t)
        Car.stop()
    else :
        wd = w.distance()
        if wd < 100:
            if temp[4] > 0.9:
                if wd > 20:
                    fmsleep(wd/50, arr = True)
                return 1
                
            else:
                aviod_chao()
        fmsleep(2)
    
    return rd


def aviod_chao():       #aviod
    w_aviod = chao()
    cot = 0
    while w_aviod.distance() < 50:
        cot = cot + 1
        print(cot)
        if w_aviod.dis_a() > 60:
            Car.right()
            time.sleep(0.5)
            Car.stop()
        elif w_aviod.dis_b() > 60:
            Car.left()
            time.sleep(0.5)
            Car.stop()
        else:
            if cot % 2 == 1:
                ret = ft('l',cot)
                if ret == 1:
                    break
            else:
                ret = ft('r', cot)
                if ret == 1:
                    break

    ret = fmsleep(2, arr=True)
    print(ret)

def ft(wh = 'l',tft = 1):
    w = chao()
    if wh == 'l':
        Car.left()
    else:
        Car.right()
    t0ft = time.time()
    time.sleep(0.00001)
    t_mpft = time.time()
    tpassft = t_mpft - t0ft
    while tpassft < tft :
        time.sleep(0.00001)
        if w.distance() > 50: 
            if w.dis_a() > 25 and w.dis_b() > 25:
                Car.stop()
                return 1
        
        t_mpft = time.time()
        tpassft = t_mpft - t0ft
    Car.stop()
    return 0

def fmsleep(tfm = 1, arr = False):
    w = chao()
    Car.forward()
    t0fm = time.time()
    time.sleep(0.00001)
    t_mpfm = time.time()
    tpassfm = t_mpfm - t0fm
    while tpassfm < tfm :
        time.sleep(0.00001)
        if arr == True:
            if w.distance() < 20:
                Car.stop()
                return 0

        elif w.distance() < 50: 
            aviod_chao()
        time.sleep(0.00001)
        if w.dis_a() < 10:
            ft('l',0.5)
        time.sleep(0.00001)
        if w.dis_b() < 10:
            ft('r',0.5)
        t_mpfm = time.time()
        tpassfm = t_mpfm - t0fm
        print(tpassfm)
    Car.stop()
    return 1
    
class chao:
    def __init__(self):
        self.GPIO_TRIGGER1 = 22 
        self.GPIO_TRIGGER2 = 38
        self.GPIO_TRIGGER3 = 32
        self.GPIO_ECHO1 = 11
        self.GPIO_ECHO2 = 36
        self.GPIO_ECHO3 = 40
        GPIO.setwarnings(False)
        GPIO.setmode(GPIO.BOARD)
        GPIO.setup(self.GPIO_TRIGGER1, GPIO.OUT)
        GPIO.setup(self.GPIO_TRIGGER2, GPIO.OUT)
        GPIO.setup(self.GPIO_TRIGGER3, GPIO.OUT)
        GPIO.setup(self.GPIO_ECHO1, GPIO.IN)
        GPIO.setup(self.GPIO_ECHO2, GPIO.IN)
        GPIO.setup(self.GPIO_ECHO3, GPIO.IN)
    
    def distance(self):
        GPIO.output(self.GPIO_TRIGGER1, True)
        time.sleep(0.00001)
        GPIO.output(self.GPIO_TRIGGER1, False)
        start_time = time.time()
        stop_time = time.time()

        while GPIO.input(self.GPIO_ECHO1) == 0:
            start_time = time.time()
            if start_time-stop_time>0.5:
                return 50

        while GPIO.input(self.GPIO_ECHO1) == 1:
            stop_time = time.time()

        time_elapsed = stop_time - start_time
        distance = (time_elapsed * 34300) / 2
        print('distance: ', distance)
        return distance

    def dis_a(self):
        GPIO.output(self.GPIO_TRIGGER2, True)
        time.sleep(0.00001)
        GPIO.output(self.GPIO_TRIGGER2, False)
        start_time = time.time()
        stop_time = time.time()
        while GPIO.input(self.GPIO_ECHO2) == 0:
            start_time = time.time()
            if start_time-stop_time>0.5:
                return 25
        while GPIO.input(self.GPIO_ECHO2) == 1:
            stop_time = time.time()
        time_elapsed = stop_time - start_time
        dis_a = (time_elapsed * 34300) / 2
        print('dis_a: ', dis_a)
        return dis_a
    
    def dis_b(self):
        GPIO.output(self.GPIO_TRIGGER3, True)
        time.sleep(0.00001)
        GPIO.output(self.GPIO_TRIGGER3, False)
        start_time = time.time()
        stop_time = time.time()
        while GPIO.input(self.GPIO_ECHO3) == 0:
            start_time = time.time()
            if start_time-stop_time>0.5:
                return 25
        while GPIO.input(self.GPIO_ECHO3) == 1:
            stop_time = time.time()
        time_elapsed = stop_time - start_time
        dis_b = (time_elapsed * 34300) / 2
        print('dis_b: ', dis_b)
        return dis_b




def _async_raise(tid, exctype):
    tid = ctypes.c_long(tid)
    if not inspect.isclass(exctype):
        exctype = type(exctype)
    res = ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, ctypes.py_object(exctype))
    if res == 0:
        raise ValueError("invalid thread id")
    elif res != 1:
        ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, None)
        raise SystemError("PyThreadState_SetAsyncExc failed")
    print('stop automode')
def stop_thread(thread):
    _async_raise(thread.ident, SystemExit)


if __name__ =='__main__':
    print ('....')
    
